#include "SteerBehaviors.h"

V2DF seek(V2DF a_posA, V2DF a_velA, V2DF a_posB, float a_maxSpeed)
{
	V2DF offset = a_posB-a_posA;
	V2DF target = offset.normal()*a_maxSpeed;
	target = target-a_velA;
	return target;
}

V2DF flee(V2DF a_posA, V2DF a_velA, V2DF a_posB, float a_maxSpeed)
{
	V2DF offset = a_posA-a_posB;
	V2DF target = offset.normal()*a_maxSpeed;
	target = target-a_velA;
	return target;
}

V2DF wander(V2DF a_pos, V2DF a_vel, float& a_wander, float a_maxSpeed)
{
	float circleRadius = 32.0f;
	float circleDistance = 60.0f;
	float amountOfChange = 0.5f;
	a_wander += randF(-amountOfChange,amountOfChange);

	V2DF circleCenter = a_vel;
	if(!circleCenter.isZero())
		circleCenter.normalize();
	circleCenter *= circleDistance;
	circleCenter += a_pos;

	V2DF offset(circleRadius*sin(a_wander),
		circleRadius*cos(a_wander));
	V2DF target = circleCenter+offset;
	target = seek(a_pos, a_vel, target, a_maxSpeed);
	return target;
}

V2DF arrival(V2DF a_posA, V2DF a_velA, V2DF a_posB,
	float a_maxSpeed, float a_slowSpeed)
{
	V2DF target;
	V2DF offset = a_posB-a_posA;
	float distance = offset.magnitude();
	float rspeed = a_maxSpeed*(distance/(a_maxSpeed*a_slowSpeed));
	float cspeed = (rspeed < a_maxSpeed)?rspeed:a_maxSpeed;
	target = offset*(cspeed/distance);
	target = target-a_velA;
	return target;
}

V2DF pursuit(V2DF a_posA, V2DF a_velA, V2DF a_posB, V2DF a_velB, float a_maxSpeed)
{
	float distance = a_posB.distance(a_posA);
	float predict = distance/a_maxSpeed;
	V2DF target = a_posB+(a_velB*predict);
	target = seek(a_posA, a_velA, target, a_maxSpeed);
	return target;
}

V2DF evade(V2DF a_posA, V2DF a_velA, V2DF a_posB, V2DF a_velB, float a_maxSpeed)
{
	float distance = a_posB.distance(a_posA);
	float predict = distance/a_maxSpeed;
	V2DF target = a_posB+(a_velB*predict);
	target = flee(a_posA, a_velA, target, a_maxSpeed);
	return target;
}